/**
 * @file test_integrated_navigation.cpp
 * @brief 
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-06-15
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */
#include <iostream>
#include <fstream>
#include <iomanip>
#include "Mechanization.hpp"
#include "IntegratedNavigation.hpp"
#include "utils.hpp"

class LnResult{
    //纬度(deg), 经度(deg), 高程(m), 北向速度(m/s), 东向速度(m/s), 地向速度(m/s), 横滚(deg), 俯仰(deg), 航向(deg)
public:
    double week;
    double second;
    Eigen::Vector3d pos;
    Eigen::Vector3d vel;
    Eigen::Vector3d att;
};

int main(int argc, char** argv){
    std::ifstream imudata_fs("/home/ghowoght/workspace/IntegratedNavigation/data/A15_imu.bin", std::ios::binary);
    Nav::ImuData imudata;
    std::ifstream gnssdata_fs("/home/ghowoght/workspace/IntegratedNavigation/data/gnss_rtk.txt");
    Nav::GnssData gnssdata;
    std::ifstream result_fs("/home/ghowoght/workspace/IntegratedNavigation/data/truth.nav");
    LnResult result;

    std::ofstream result_ofs("/home/ghowoght/workspace/IntegratedNavigation/data/result.nav");

    do{
        imudata_fs.read((char*)(&imudata), sizeof(imudata));
    }while(imudata.timestamp < Nav::START_TIME);

    do{
        result_fs   >> result.week >> result.second
                    >> result.pos[0] >> result.pos[1] >> result.pos[2]
                    >> result.vel[0] >> result.vel[1] >> result.vel[2]
                    >> result.att[0] >> result.att[1] >> result.att[2];
    }while(result.second < Nav::START_TIME);
    gnssdata.pos[0] *= Nav::D2R;
    gnssdata.pos[1] *= Nav::D2R;

    do{
        gnssdata_fs >> gnssdata.timestamp
                    >> gnssdata.pos[0] >> gnssdata.pos[1] >> gnssdata.pos[2]
                    >> gnssdata.pos_std[0] >> gnssdata.pos_std[1] >> gnssdata.pos_std[2];
    }while(gnssdata.timestamp < Nav::START_TIME);

    /* IMU参数 */
    // 转换为标准单位 m/s/rad
    Nav::ImuModel imumodel;
    imumodel.ARW                          = 0.003 * Nav::D2R / 60.0;  
    imumodel.VRW                          = 0.03 / 60.0;   
    imumodel.gyro_bias_std                = 0.027 * Nav::D2R / 3600.0;  
    imumodel.gyro_bias_corr_time          = 4 * 3600.0;     
    imumodel.accel_bias_std               = 15 * 1e-5;     
    imumodel.accel_bias_corr_time         = 4 * 3600.0;      
    imumodel.gyro_scale_factor_std        = 300 * 1e-6;    
    imumodel.gyro_scale_factor_corr_time  = 4 * 3600.0;      
    imumodel.accel_scale_factor_std       = 300 * 1e-6;    
    imumodel.accel_scale_factor_corr_time = 4 * 3600.0;     
	
    /* 初始状态 */   
    auto state = std::make_shared<Nav::State>();
    state->timestamp = imudata.timestamp;
    state->pos << Nav::D2R * 30.444787369, Nav::D2R * 114.471863247, 20.910;
    state->vel << 0, 0, 0;
    state->att << Nav::D2R * 185.696, Nav::D2R * -2.0345, Nav::D2R * 0.854; // yaw pit rol
    state->pos_std << 0.005, 0.004, 0.008;
    state->vel_std << 0.003, 0.004, 0.004;
    state->att_std << Nav::D2R * 0.023, Nav::D2R * 0.003, Nav::D2R * 0.003;

    Nav::DataPtr data_raw = std::make_shared<Nav::Data>();
    data_raw->imudata = imudata;
    data_raw->gnssdata = gnssdata;

    Nav::IntegratedNavigation inteNav;
    inteNav.initialize(state, data_raw, imumodel);

    while(!imudata_fs.eof()){
    // for(int i = 0; i < 300000; i++){
        imudata_fs.read((char*)(&imudata), sizeof(imudata));
        if(!data_raw->gnssdata.valid){            
            gnssdata_fs >> gnssdata.timestamp
                        >> gnssdata.pos[0] >> gnssdata.pos[1] >> gnssdata.pos[2]
                        >> gnssdata.pos_std[0] >> gnssdata.pos_std[1] >> gnssdata.pos_std[2];
            gnssdata.valid = true;
            gnssdata.pos[0] *= Nav::D2R;
            gnssdata.pos[1] *= Nav::D2R;
        }        
        data_raw->imudata  = imudata;
        data_raw->gnssdata = gnssdata;
        inteNav.KalmanFilter(data_raw);

        LnResult result_last = result;
        LnResult result_curr = result;
        if(1){
            /* 和参考结果对齐 */
            do{
                result_fs   >> result_curr.week >> result_curr.second
                            >> result_curr.pos[0] >> result_curr.pos[1] >> result_curr.pos[2]
                            >> result_curr.vel[0] >> result_curr.vel[1] >> result_curr.vel[2]
                            >> result_curr.att[0] >> result_curr.att[1] >> result_curr.att[2];
                if(result_last.second < state->timestamp && result_curr.second > state->timestamp){
                    auto ratio = (state->timestamp - result_last.second) / (result_curr.second - result_last.second);
                    result.second = (result_curr.second - result_last.second) * ratio + result_last.second;
                    result.pos = (result_curr.pos - result_last.pos) * ratio + result_last.pos;
                    result.vel = (result_curr.vel - result_last.vel) * ratio + result_last.vel;
                    result.att = (result_curr.att - result_last.att) * ratio + result_last.att;
                    break;
                }
                result_last = result_curr;
            }while(true && !result_fs.eof());

            result_ofs.precision(12);
            result_ofs << result.week << " " << state->timestamp << " ";
            result_ofs << Nav::R2D * state->pos[0] << " " << Nav::R2D * state->pos[1] << " " << state->pos[2] << " ";
            result_ofs << state->vel[0] << " " << state->vel[1] << " " << state->vel[2] << " ";
            auto yaw = Nav::R2D * state->att[Nav::YAW] + 360;
            yaw = yaw >= 360 ? yaw - 360 : yaw;
            result_ofs << Nav::R2D * state->att[Nav::ROL] << " " << Nav::R2D * state->att[Nav::PIT] << " " << yaw << std::endl;


            

            std::cout.precision(12);
            /* 位置 */
            std::cout << "--compute: ";
            std::cout   << Nav::R2D * state->pos[0] << " " 
                        << Nav::R2D * state->pos[1] << " " 
                        << state->pos[2] << std::endl;
            std::cout << "--ref    : ";
            std::cout   << result.pos[0] << " " 
                        << result.pos[1] << " " 
                        << result.pos[2] << std::endl;
            std::cout << "-----------" << std::endl;
            /* 速度 */
            // std::cout << "--compute: ";
            // std::cout   << state->vel[Nav::N] << " " 
            //             << state->vel[Nav::E] << " " 
            //             << state->vel[Nav::D]<< std::endl;
            // std::cout << "--ref    : ";
            // std::cout   << result.vel[0] << " " 
            //             << result.vel[1] << " " 
            //             << result.vel[2] << std::endl;
            // std::cout << "-----------" << std::endl;
            /* 姿态 */
            // std::cout << "--compute: ";
            // std::cout   << Nav::R2D * state->att[Nav::ROL] << " " 
            //             << Nav::R2D * state->att[Nav::PIT] << " " 
            //             << yaw << std::endl;
            // std::cout << "--ref    : ";
            // std::cout   << result.att[0] << " " 
            //             << result.att[1] << " "  
            //             << result.att[2] << std::endl;
            // std::cout << "-----------" << std::endl;
        }
    }
    imudata_fs.close();
    gnssdata_fs.close();
}

